﻿using Engine;

namespace MFTrain
{
    public static class ExMaths
    {
        public static Vector3 ToForwardVector(this Quaternion quaternion)
        {
            return new Vector3((float)(-2.0 * ((double)quaternion.Y * (double)quaternion.W + (double)quaternion.Y * (double)quaternion.W)), (float)(2.0 * ((double)quaternion.Y * (double)quaternion.W - (double)quaternion.Y * (double)quaternion.W)), (float)(2.0 * ((double)quaternion.Y * (double)quaternion.W + (double)quaternion.Y * (double)quaternion.W) - 1.0));
        }

        public static Vector3 ToForwardVector2(this Quaternion quaternion)
        {
            float x = 2f * (quaternion.X * quaternion.Z + quaternion.W * quaternion.Y);
            float y = 2f * (quaternion.Y * quaternion.Z - quaternion.W * quaternion.X);
            float z = 1f - 2f * (quaternion.X * quaternion.X + quaternion.Y * quaternion.Y);
            return -Vector3.Normalize(new Vector3(x, y, z));
        }
    }
}